function [out, auxOut, in] = intnav_kfvld(in, cfg, par, iniCon, cst) %#codegen
%INTNAV_KFVLD 用于卡尔曼滤波验证的组合导航程序
%
% Input Arguments:
% in.INS.accOut: x*3矩阵，输入为加速度计比速度增量，输出为经卡尔曼滤波器补偿后的比速度增量，单位m/s/dT
% in.INS.gyroOut: x*3矩阵，输入为陀螺角增量，输出为经卡尔曼滤波器补偿后的角增量，单位rad/dT
% in.aid.CNE: 3*3*x矩阵，aid位置矩阵
% in.aid.vN: x*3矩阵，N系下的aid速度，单位m/s
% in.aid.vG: x*3矩阵，G系下的aid速度，单位m/s
% in.aid.CBN: 3*3*x矩阵，aid姿态矩阵
% in.aid.h: x*1矩阵，aid高度，单位为m
% in.aid.alpha: x*1矩阵，游动方位角，单位rad
% in.traj: 结构体，理想导航参数（用于状态转移矩阵的计算，若不能提供，则置为空），应为本卡尔曼滤波解算周期结束时刻值，单位均为国际单位制单位
% in.u: m*1列向量，控制向量，与状态向量同维，不为空时引入控制向量
% cfg.enableKalmFilt: 逻辑标量，true表示使能卡尔曼滤波器，false表示使用纯惯性导航
% cfg.enableKFUpdate: 逻辑标量，true表示使能卡尔曼滤波更新周期，false表示不使能卡尔曼滤波更新周期，cfg.enableKalmFilt为true时有效
% cfg.extrapAlgo: 枚举标量，外推算法，KalmFiltExtrapAlgo.ITER采用迭代型误差协方差外推计算式，其他值采用普通的误差协方差外推计算式
% cfg.updAlgo: 枚举标量，更新算法，KalmFiltUpdAlgo.JOSEPH采用约瑟夫型误差协方差更新计算式，其他值采用普通的误差协方差更新计算式
% cfg.useDCMInAttCalc: 逻辑标量，true表示在姿态解算中使用方向余弦矩阵，false表示使用四元数
% cfg.dynaMatINSSysBlkForm: 标量，1采用φ形式的惯性导航误差方程，其它值采用ψ形式的惯性导航误差方程
% cfg.navCoordType: 枚举标量，NavCoordType.GEO为ENU地理坐标系，NavCoordType.WAN_AZM为游动方位坐标系，NavCoordType.FREE_AZM为自由方位坐标系
% cfg.sysStateMask: 1*11逻辑向量，true表示包含该系统状态，详细状态为第1、2、10列元素为位置误差，第3列元素为高度误差，第4-6列元素为速度误差，第7-9列元素为姿态误差，第11列为deVC3
% cfg.sensStateMask: 1*36逻辑向量，true表示包含该器件状态，器件状态依次包含加速度计的3个零偏误差、3个标度因数误差、6个失准角误差、3个二次项系数误差和陀螺的3个零偏误差、3个标度因数误差、6个失准角误差、9个g敏感项误差
% cfg.enableNumCondCtrl: 逻辑标量，true表示在计算状态误差协方差矩阵P时进行数值条件控制，使P阵保持对称性及非负定性，防止滤波发散，false表示不进行数值条件控制
% par.tBgn: 标量，组合导航起始时刻，单位s
% par.tEnd: 标量，组合导航结束时刻，单位s
% par.dT: 标量，采样周期，单位s
% par.tUpd: 标量，表示aid组合导航的量测更新周期，单位s
% par.g: 标量，重力加速度，单位m/s^2
% par.INSCycPerKalFiltExtrapCyc: int32标量，表示卡尔曼滤波器外推周期中包含的导航周期数
% par.GammaMRGammaMT: n*n矩阵，n为观测量维数，等价于GammaM*R*GammaMT，其中GammaM为量测噪声耦合矩阵，R为量测噪声序列协方差矩阵，GammaMT为GammaM的转置
% par.INSSensRWC: 6*1向量，分别为3个加速度计的速度随机游走系数和3个陀螺的角度随机游走系数，用于速度误差变化率和姿态误差变化率过程噪声方差计算，单位为国际单位制单位
% par.INSSensStateProcNoisePSDFull: 36*1向量，分别为对应的完整器件状态的过程噪声的功率谱密度，这些状态均采用一阶马尔柯夫过程描述，单位为国际单位制单位
% par.PDiagMin: m*1列向量，各元素为卡尔曼滤波状态误差协方差矩阵P对角线元素对应的合理最小值
% par.PDiagMax: m*1列向量，各元素为卡尔曼滤波状态误差协方差矩阵P对角线元素对应的合理最大值
% iniCon.pos: 3*1向量，初始位置，各元素分别为纬度、经度、高度，单位分别为rad、rad、m
% iniCon.vN: 3*1向量，初始速度，单位m/s
% iniCon.CBN: 3*3矩阵，初始姿态，方向余弦矩阵
% iniCon.kalmFilt.x: m*1列向量，状态向量初始值
% iniCon.kalmFilt.P: m*m矩阵，状态误差协方差矩阵初始值
% cst.RE: 标量，地球长半轴，单位m
% cst.e: 标量，地球椭圆度，无单位
% cst.mu: 标量，地球质量与万有引力常数之积，单位m^3/s^2
% cst.J2, cst.J3: 标量，与地球质量分布相关的常量，无单位
% cst.omegaIE: 标量，地球自转角速率，单位rad/s
% cst.epsilonSq: 标量，地球偏心率的平方，无单位
%
% Output Arguments:
% out.t: m*1向量，m约为从粗对准结束时刻到导航结束时刻的惯组采样数，导航时间，单位s
% out.CNE: 3*3*m矩阵，导航位置矩阵
% out.pos: m*3矩阵，导航位置，各列分别为纬度、经度、高度，单位分别为rad、rad、m
% out.vN: m*3矩阵，N系下的导航速度，单位为m/s
% out.vG: m*3矩阵，G系下的导航速度，各列分别为东、北、天向速度，单位为m/s
% out.CBL: 3*3*m矩阵，导航姿态矩阵
% out.CBN: 3*3*m矩阵，导航姿态矩阵
% out.CBG: 3*3*m矩阵，导航姿态矩阵
% out.att: m*3矩阵，导航姿态，各列分别为滚动、俯仰、航向角度，单位为rad
% out.alpha: m*1向量，游动方位角，单位rad
% out.gN: 3*m向量，导航系下的重力加速度，单位m/s^2
% out.FCN: 3*3*m矩阵，曲率矩阵
% out.kalmFilt_tExtrap: p*1向量，p约为从粗对准结束时刻到导航结束时刻的卡尔曼滤波外推周期数，卡尔曼滤波器外推时间，单位s
% out.kalmFilt_x: p*q矩阵，q为卡尔曼滤波器状态向量长度，卡尔曼滤波器状态向量（更新前）
% out.kalmFilt_PDiagSqrt: p*q矩阵，卡尔曼滤波器状态误差协方差矩阵对角线平方根（更新前）
% out.kalmFilt_tUpd: m*1向量，m约为从粗对准结束时刻到导航结束时刻的卡尔曼滤波更新周期数，卡尔曼滤波器更新时间，单位s
% out.kalmFilt_xPost: m*q矩阵，卡尔曼滤波器状态向量（更新后，施加控制前）
% out.kalmFilt_PPostDiagSqrt: m*q矩阵，卡尔曼滤波器状态误差协方差矩阵对角线平方根（更新后）
% auxOut.kalmFilt_Phi: q*q*(p-1)矩阵，卡尔曼滤波器状态转移矩阵
% auxOut.kalmFilt_P: q*q*p矩阵，卡尔曼滤波器状态误差协方差矩阵（更新前）
% auxOut.kalmFilt_PPost: q*q*m矩阵，卡尔曼滤波器状态误差协方差矩阵（更新后）
% auxOut.kalmFilt_K: q*y*m矩阵，y为观测向量最大长度，卡尔曼滤波器增益矩阵
% auxOut.kalmFilt_zObs: m*y矩阵，卡尔曼滤波器观测向量
% auxOut.kalmFilt_measResid: m*y矩阵，量测向量残差
% auxOut.kalmFilt_measResidApplyUc: m*y矩阵，引入控制向量后的量测向量残差
% auxOut.kalmFilt_Q: q*q*p矩阵，卡尔曼滤波器过程噪声序列协方差矩阵（更新前）

%% 常量
cst_CNL = [0 1 0; 1 0 0; 0 0 -1];
cst_SINI = struct('RE', cst.RE, 'e', cst.e, 'mu', cst.mu, 'J2', cst.J2, 'J3', cst.J3, 'omegaIE', cst.omegaIE);

%% 配置
% sinistep_pgs函数配置
cfg_SINI.NSInLS = 1;
cfg_SINI.HSInNS = 1;
cfg_SINI.useDCMInAttCalc = cfg.useDCMInAttCalc;
cfg_SINI.useNormOrthoInAttCalc = true;
cfg_SINI.useHiResPosNSCal = true;
cfg_SINI.useExtGravity = true;
cfg_SINI.navCoordType = cfg.navCoordType;
% kalmfiltstep函数配置
cfg_kalmFilt.nPhilambdalambda = 2;
cfg_kalmFilt.nPhilambday = 2;
cfg_kalmFilt.extrapAlgo = cfg.extrapAlgo;
cfg_kalmFilt.updAlgo = cfg.updAlgo;
cfg_kalmFilt.enableNumCondCtrl = cfg.enableNumCondCtrl;
cfg_kalmFilt.enableMeasResidChk = false;

%% 参量
% sinistep_pgs函数参数
par_SINI.Tl = par.dT;
par_SINI.g = par.g;
par_SINI.C = [0, 0, 0];

% kalmfiltstep函数参数
par_kalmFilt.dTExtrap = par.dT * double(par.INSCycPerKalFiltExtrapCyc);
par_kalmFilt.PDiagMin = par.PDiagMin;
par_kalmFilt.PDiagMax = par.PDiagMax;

%% 中间量初始化
mid_DTGgSensSymmetry = zeros(1, 9, 'int8');
mid_obsVecLength = size(par.GammaMRGammaMT, 1);
mid_sysStateNum = nnz(cfg.sysStateMask);
mid_sensStateNum = nnz(cfg.sensStateMask);
mid_stateNum = mid_sysStateNum + mid_sensStateNum;
mid_INSDataBgnIdx = int32(1); % 该序号惯组采样的起始时刻为tBgn
mid_INSDataEndIdx = int32(round((par.tEnd-par.tBgn) / par.dT)); % 该序号惯组采样的结束时刻为tEnd
tmp_INSSampPerKalFiltUpd = int32(round(par.tUpd/par.dT));
mid_INSDataKFUpdIdx = mid_INSDataBgnIdx-1:tmp_INSSampPerKalFiltUpd:mid_INSDataEndIdx; % 在该序号惯组采样的结束时刻上进行卡尔曼滤波更新

%% 初始条件
% sinistep_pgs函数初始条件
iniCon_SINI.L_n = iniCon.pos(1); % 0时刻量，rad
iniCon_SINI.L_n1 = iniCon.pos(1); % -1n时刻量，rad
iniCon_SINI.lambda_n = iniCon.pos(2); % 0时刻量，rad
iniCon_SINI.lambda_n1 = iniCon.pos(2); % -1n时刻量，rad
iniCon_SINI.h_n = iniCon.pos(3); % 0时刻量，m
iniCon_SINI.h_n1 = iniCon.pos(3); % -1n时刻量，m
iniCon_SINI.vN_m = iniCon.vN; % 0时刻量
iniCon_SINI.vN_m1 = iniCon.vN; % -1m时刻量
iniCon_SINI.vN_n1 = iniCon.vN; % -1n时刻量
iniCon_SINI.CBL = cst_CNL * iniCon.CBN;
iniCon_SINI.specVelInc_lx = in.INS.accOut(mid_INSDataBgnIdx, :)'; % 0时刻之前的比速度增量，第1列为-1时刻~0时刻周期，第2列为-2时刻~-1时刻周期，依此类推
iniCon_SINI.angleInc_lx = in.INS.gyroOut(mid_INSDataBgnIdx, :)'; % 0时刻之前的角度增量，第1列为-1时刻~0时刻周期，第2列为-2时刻~-1时刻周期，依此类推
% kalmfiltstep函数初始条件
iniCon_kalmFilt.x = iniCon.kalmFilt.x;
iniCon_kalmFilt.P = iniCon.kalmFilt.P;

%% 输出量初始化
% 导航周期相关量
out.t = NaN(mid_INSDataEndIdx-mid_INSDataBgnIdx+2, 1);
out.CNE = NaN(3, 3, mid_INSDataEndIdx-mid_INSDataBgnIdx+2);
out.pos = NaN(mid_INSDataEndIdx-mid_INSDataBgnIdx+2, 3);
out.vN = NaN(mid_INSDataEndIdx-mid_INSDataBgnIdx+2, 3);
out.vG = NaN(mid_INSDataEndIdx-mid_INSDataBgnIdx+2, 3);
out.CBL = NaN(3, 3, mid_INSDataEndIdx-mid_INSDataBgnIdx+2);
out.CBN = NaN(3, 3, mid_INSDataEndIdx-mid_INSDataBgnIdx+2);
out.CBG = NaN(3, 3, mid_INSDataEndIdx-mid_INSDataBgnIdx+2);
out.att = NaN(mid_INSDataEndIdx-mid_INSDataBgnIdx+2, 3);
out.alpha = NaN(mid_INSDataEndIdx-mid_INSDataBgnIdx+2, 1);
out.gN = NaN(mid_INSDataEndIdx-mid_INSDataBgnIdx+2, 3);
out.FCN = NaN(3, 3, mid_INSDataEndIdx-mid_INSDataBgnIdx+2);
% 外推周期相关量
tmp_KFExtrapCycNum = floor(double(mid_INSDataEndIdx-mid_INSDataBgnIdx+1)/double(par.INSCycPerKalFiltExtrapCyc))+1;
if cfg.enableKalmFilt
    out.kalmFilt_tExtrap = NaN(tmp_KFExtrapCycNum, 1);
    out.kalmFilt_x = NaN(tmp_KFExtrapCycNum, mid_stateNum);
    out.kalmFilt_PDiagSqrt = NaN(tmp_KFExtrapCycNum, mid_stateNum);
    auxOut.kalmFilt_Phi = NaN(mid_stateNum, mid_stateNum, tmp_KFExtrapCycNum-1);
    auxOut.kalmFilt_P = NaN(mid_stateNum, mid_stateNum, tmp_KFExtrapCycNum);
    auxOut.kalmFilt_Q = NaN(mid_stateNum, mid_stateNum, tmp_KFExtrapCycNum-1);
else
    out.kalmFilt_tExtrap = NaN(0, 1);
    out.kalmFilt_x = NaN(0, mid_stateNum);
    out.kalmFilt_PDiagSqrt = NaN(0, mid_stateNum);
    auxOut.kalmFilt_Phi = NaN(mid_stateNum, mid_stateNum, 0);
    auxOut.kalmFilt_P = NaN(mid_stateNum, mid_stateNum, 0);
    auxOut.kalmFilt_Q = NaN(mid_stateNum, mid_stateNum, 0);
end
% 更新周期相关量
if cfg.enableKFUpdate
    out.kalmFilt_tUpd = NaN(size(mid_INSDataKFUpdIdx, 2)-1, 1);
    out.kalmFilt_xPost = NaN(size(mid_INSDataKFUpdIdx, 2)-1, mid_stateNum);
    out.kalmFilt_PPostDiagSqrt = NaN(size(mid_INSDataKFUpdIdx, 2)-1, mid_stateNum);
    auxOut.kalmFilt_PPost = NaN(mid_stateNum, mid_stateNum, size(mid_INSDataKFUpdIdx, 2)-1);
    auxOut.kalmFilt_K = NaN(mid_stateNum, mid_obsVecLength, size(mid_INSDataKFUpdIdx, 2)-1);
    auxOut.kalmFilt_zObs = NaN(size(mid_INSDataKFUpdIdx, 2)-1, mid_obsVecLength);
    auxOut.kalmFilt_measResid = NaN(size(mid_INSDataKFUpdIdx, 2)-1, mid_obsVecLength);
    auxOut.kalmFilt_measResidApplyUc = NaN(size(mid_INSDataKFUpdIdx, 2)-1, mid_obsVecLength);
else
    out.kalmFilt_tUpd = NaN(0, 1);
    out.kalmFilt_xPost = NaN(0, mid_stateNum);
    out.kalmFilt_PPostDiagSqrt = NaN(0, mid_stateNum);
    auxOut.kalmFilt_PPost = NaN(mid_stateNum, mid_stateNum, 0);
    auxOut.kalmFilt_K = NaN(mid_stateNum, mid_obsVecLength, 0);
    auxOut.kalmFilt_zObs = NaN(tmp_KFExtrapCycNum, mid_obsVecLength);
    auxOut.kalmFilt_measResid = NaN(0, mid_obsVecLength);
    auxOut.kalmFilt_measResidApplyUc = NaN(0, mid_obsVecLength);
end

%% 运行
for i=0:mid_INSDataEndIdx-mid_INSDataBgnIdx+1 % =0时初始化，>0时为解算周期
    if i == 0
        mid_SINIMode = 0;
        mid_kalmFiltMode = 0;
        mid_INSDataIdx = mid_INSDataBgnIdx; % 惯组采样的时刻
    else
        mid_SINIMode = 1;
        mid_kalmFiltMode = 1;
        mid_INSDataIdx = mid_INSDataBgnIdx+i-1;
    end
    mid_t = par.tBgn + double(i)*par.dT;
    
    % 导航解算周期
    in_SINI = struct('CNEExt', NaN(3), 'hExt', NaN, 'vNExt', NaN(3, 1), 'CBLExt', NaN(3), 'qBLExt', NaN(1, 4), 'hAltm', 0, ...
        'specVelInc', in.INS.accOut(mid_INSDataIdx, :)', 'angleInc', in.INS.gyroOut(mid_INSDataIdx, :)');
    [out_SINI] = sinistep_pgs(mid_SINIMode, in_SINI, cfg_SINI, par_SINI, iniCon_SINI, cst_SINI);
    
    % 卡尔曼滤波解算周期
    if cfg.enableKalmFilt
        if mod(i, par.INSCycPerKalFiltExtrapCyc) == 0
            % 外推
            mid_FBlk.traj.CNE = in.traj.CNE(:, :, i+1);
            mid_FBlk.traj.pos = in.traj.pos(i+1, :)';
            mid_FBlk.traj.vN = in.traj.vN(i+1, :)';
            mid_FBlk.traj.CBN = in.traj.CBN(:, :, i+1);
            mid_FBlk.traj.fB = in.traj.fB(i+1, :)';
            mid_FBlk.traj.omegaIBB = in.traj.omegaIBB(i+1, :)';
            mid_FBlk.traj.fN = in.traj.CBN(:, :, i+1) * in.traj.fB(i+1, :)';
            mid_FBlk.traj.FCN = in.traj.FCN(:, :, i+1);
            mid_FBlk.traj.g = norm(in.traj.gN(i+1, :));
            if cfg.dynaMatINSSysBlkForm == 1
                mid_FINSBlk = [naverrdynamat_inssysblk_phiform(mid_FBlk.traj.CNE, mid_FBlk.traj.vN, mid_FBlk.traj.fN, mid_FBlk.traj.pos(1, 1), mid_FBlk.traj.pos(3, 1),...
                    mid_FBlk.traj.g, mid_FBlk.traj.FCN, par_SINI.C, cst.omegaIE, cfg.navCoordType, false, cfg.sysStateMask), ...
                    naverrdynamat_inssens2sysblk(mid_FBlk.traj.CBN, mid_FBlk.traj.fB, mid_FBlk.traj.omegaIBB, cfg.sysStateMask, cfg.sensStateMask, mid_DTGgSensSymmetry);
                    zeros(mid_sensStateNum, mid_sysStateNum), zeros(mid_sensStateNum, mid_sensStateNum)];
            else
                mid_FINSBlk = [naverrdynamat_inssysblk_psiform(mid_FBlk.traj.CNE, mid_FBlk.traj.vN, mid_FBlk.traj.fN, mid_FBlk.traj.pos(1, 1), mid_FBlk.traj.pos(3, 1), ...
                    mid_FBlk.traj.g, mid_FBlk.traj.FCN, par_SINI.C, cst.omegaIE, cfg.navCoordType, false, cfg.sysStateMask), ...
                    naverrdynamat_inssens2sysblk(mid_FBlk.traj.CBN, mid_FBlk.traj.fB, mid_FBlk.traj.omegaIBB, cfg.sysStateMask, cfg.sensStateMask, mid_DTGgSensSymmetry);
                    zeros(mid_sensStateNum, mid_sysStateNum), zeros(mid_sensStateNum, mid_sensStateNum)];
            end
            in_kalmFilt.F = [mid_FINSBlk, NaN(mid_stateNum, 0); NaN(0, mid_stateNum), NaN(0, 0)];
            in_kalmFilt.GPQGPT = kfprocnoisepsdmat(mid_FBlk.traj.CBN, par.INSSensRWC, par.INSSensStateProcNoisePSDFull,...
                NaN(0, 1), NaN(0, 0), cfg.sysStateMask, cfg.sensStateMask);
            in_kalmFilt.zObs = NaN(mid_obsVecLength, 1);
            in_kalmFilt.H = NaN(mid_obsVecLength, mid_stateNum);
            in_kalmFilt.GammaMRGammaMT = par.GammaMRGammaMT;
            in_kalmFilt.u = NaN(mid_stateNum, 1);
            
            [out_kalmFilt, auxOut_kalmFilt] = kalmfiltstep(mid_kalmFiltMode, in_kalmFilt, cfg_kalmFilt, par_kalmFilt, iniCon_kalmFilt);
            out.kalmFilt_tExtrap(out_kalmFilt.extrapCycCnt+1, :) = mid_t;
            out.kalmFilt_x(out_kalmFilt.extrapCycCnt+1, :) = out_kalmFilt.x';
            out.kalmFilt_PDiagSqrt(out_kalmFilt.extrapCycCnt+1, :) = reshape(sqrt(diag(out_kalmFilt.P)'), 1, mid_stateNum);
            if out_kalmFilt.extrapCycCnt ~= 0
                auxOut.kalmFilt_Phi(:, :, out_kalmFilt.extrapCycCnt) = auxOut_kalmFilt.Phi;
                auxOut.kalmFilt_Q(:, :, out_kalmFilt.extrapCycCnt) = auxOut_kalmFilt.GammaPQGammaPT;
            end
            auxOut.kalmFilt_P(:, :, out_kalmFilt.extrapCycCnt+1) = out_kalmFilt.P;
            
            % 更新
            if cfg.enableKFUpdate && (i~=0)
                if any(mid_INSDataKFUpdIdx==(i+mid_INSDataBgnIdx-1))
                    mid_aidDataKFUpdIdx = int32(round((mid_t-par.tBgn) / par.dT) + 1); % 该序号aid采样时刻为更新时刻
                    in_kalmFilt.zObs = calczobs(in.aid, out_SINI, mid_aidDataKFUpdIdx, mid_obsVecLength, iniCon.kalmFilt.x, cfg.dynaMatINSSysBlkForm);
                    in_kalmFilt.H = eye(size(in_kalmFilt.zObs, 1), mid_stateNum);
                    [out_kalmFilt, auxOut_kalmFilt] = kalmfiltstep(2, in_kalmFilt, cfg_kalmFilt, par_kalmFilt, iniCon_kalmFilt);
                    out.kalmFilt_tUpd(out_kalmFilt.updCnt, :) = mid_t;
                    out.kalmFilt_xPost(out_kalmFilt.updCnt, :) = out_kalmFilt.x';
                    out.kalmFilt_PPostDiagSqrt(out_kalmFilt.updCnt, :) = reshape(sqrt(diag(out_kalmFilt.P)'), 1, mid_stateNum);
                    auxOut.kalmFilt_PPost(:, :, out_kalmFilt.updCnt) = out_kalmFilt.P;
                    auxOut.kalmFilt_K(:, 1:size(in_kalmFilt.zObs, 1), out_kalmFilt.updCnt) = auxOut_kalmFilt.K;
                    auxOut.kalmFilt_zObs(out_kalmFilt.updCnt, 1:size(in_kalmFilt.zObs, 1)) = in_kalmFilt.zObs';
                    auxOut.kalmFilt_measResid(out_kalmFilt.updCnt, 1:size(in_kalmFilt.zObs, 1)) = auxOut_kalmFilt.measResid;
                    
                    if ~isempty(in.u) % 用于f步骤验证
                        % 修正导航参数
                        mid_u = zeros(10, 1);
                        mid_u(cfg.sysStateMask, 1) = -in.u(1:mid_sysStateNum, 1);
                        if cfg.dynaMatINSSysBlkForm == 1
                            if cfg_SINI.useDCMInAttCalc
                                [out_SINI.CNE, out_SINI.h, tmp_vN, out_SINI.CBL] = cornavparams_phiform_dcm(out_SINI.CNE, out_SINI.h, out_SINI.vN', out_SINI.CBL, mid_u');
                            else
                                [out_SINI.CNE, out_SINI.h, tmp_vN, out_SINI.qBL] = cornavparams_phiform_quat(out_SINI.CNE, out_SINI.h, out_SINI.vN', out_SINI.qBL, mid_u');
                                out_SINI.CBL = quat2dcm_cg(out_SINI.qBL)';
                            end
                        else
                            if cfg_SINI.useDCMInAttCalc
                                [out_SINI.CNE, out_SINI.h, tmp_vN, out_SINI.CBL] = cornavparams_psiform_dcm(out_SINI.CNE, out_SINI.h, out_SINI.vN', out_SINI.CBL, mid_u');
                            else
                                [out_SINI.CNE, out_SINI.h, tmp_vN, out_SINI.qBL] = cornavparams_psiform_quat(out_SINI.CNE, out_SINI.h, out_SINI.vN', out_SINI.qBL, mid_u');
                                out_SINI.CBL = quat2dcm_cg(out_SINI.qBL)';
                            end
                        end
                        [out_SINI.L, out_SINI.lambda, tmp_vG, out_SINI.CBN, out_SINI.CBG, out_SINI.phi, out_SINI.theta, ~, out_SINI.psiTrue, out_SINI.alpha] = wholenavparam(out_SINI.CNE, tmp_vN, out_SINI.CBL);
                        out_SINI.vN = tmp_vN';
                        out_SINI.vG = tmp_vG';
                        mid_zHat = in_kalmFilt.H*(out.kalmFilt_x(out_kalmFilt.extrapCycCnt+1, :)' + in.u); % kalmfiltstep中计算auxOut.kalmFilt_measResid所用out_kalmFilt.x为更新前的状态，此处和其保持一致
                        mid_zObs = calczobs(in.aid, out_SINI, mid_aidDataKFUpdIdx, mid_obsVecLength, iniCon.kalmFilt.x, cfg.dynaMatINSSysBlkForm);
                        auxOut.kalmFilt_measResidApplyUc(out_kalmFilt.updCnt, 1:size(in_kalmFilt.zObs, 1)) = mid_zObs - mid_zHat;
                    end
                end
            else
                % 用于卡尔曼滤波验证步骤a，计算观测向量
                mid_aidDataKFzObsIdx = int32(round((mid_t-par.tBgn) / par.dT) + 1);
                tmp_zObs = calczobs(in.aid, out_SINI, mid_aidDataKFzObsIdx, mid_obsVecLength, iniCon.kalmFilt.x, cfg.dynaMatINSSysBlkForm);
                auxOut.kalmFilt_zObs(out_kalmFilt.extrapCycCnt+1, 1:size(in_kalmFilt.zObs, 1)) = tmp_zObs;
            end
        end
    end
    out.t(i+1, :) = mid_t;
    out.CNE(:, :, i+1) = out_SINI.CNE;
    out.pos(i+1, :) = [out_SINI.L, out_SINI.lambda, out_SINI.h];
    out.vN(i+1, :) = out_SINI.vN';
    out.vG(i+1, :) = out_SINI.vG';
    out.CBL(:, :, i+1) = out_SINI.CBL;
    out.CBN(:, :, i+1) = out_SINI.CBN;
    out.CBG(:, :, i+1) = out_SINI.CBG;
    out.att(i+1, :) = [out_SINI.phi, out_SINI.theta, out_SINI.psiTrue];
    out.alpha(i+1) = out_SINI.alpha;
    out.gN(i+1, :) = out_SINI.gN';
    out.FCN(:, :, i+1) = out_SINI.FCN;
end
end

function [zObs] = calczobs(aid, INS, updIdx, obsVecLength, iniCon_x, dynaMatINSSysBlkForm)
%CALCZOBS 计算观测向量
mid_hAid = aid.h(updIdx, :)';
mid_vNAid = aid.vN(updIdx, :)';
mid_vGAid = aid.vG(updIdx, :)';
mid_CBNAid = aid.CBN(:, :, updIdx);
mid_CENAid = aid.CNE(:, :, updIdx)';
mid_alphaAid = aid.alpha(updIdx, :)';
dthetaN = dcmdiff(INS.CNE', mid_CENAid);
dh = INS.h - mid_hAid;
dvNN = kfobsvec_vel(INS.vN, mid_vGAid, mid_alphaAid);
phiN = dcmdiff(INS.CBN, mid_CBNAid);
if dynaMatINSSysBlkForm == 1
    zObsSys = [dthetaN(1, 1:2)'; dh; dvNN; phiN'; dthetaN(1, 3)];
else
    dvEN = dvNN + cross(dthetaN', mid_vNAid);
    psiN = phiN - dthetaN;
    zObsSys = [dthetaN(1, 1:2)'; dh; dvEN; psiN'; dthetaN(1, 3)];
end
if obsVecLength == 37 % 全维
    zObsSens = iniCon_x(11:end); % 步骤a、b、c中过程噪声为0，器件误差的观测可以认为是定值
    zObs = [zObsSys; zObsSens];
elseif obsVecLength == 10 % 仅观测系统状态
    zObs = zObsSys;
elseif obsVecLength == 6 % 仅观测位置+速度
    zObs = zObsSys(1:6, :);
elseif obsVecLength == 3 % 仅观测位置
    zObs = zObsSys(1:3, :);
else
    error('calczobs:obsVecLength', 'obsVecLength error.');
end
end